// Device_Motor_FuYu.cpp
#include "Device_Motor_FuYu.h"
#include <QDebug>
#include "FMC4030-Dll.h"
#include <cmath>
#include "../Device_Motor_Panel/MotorConfig.h"
#pragma comment(lib, "FMC4030-Dll.lib")

    Device_Motor_FuYu::Device_Motor_FuYu(QObject* parent)
        : Device_Motor(MotorType::Device_Motor_FuYu, parent),
      m_deviceId(0), m_axis(0), m_port(8088),
      m_currentPosition(0.0), m_isConnected(false)
    {
    m_ipAddress = "192.168.1.100";
    }

    Device_Motor_FuYu::~Device_Motor_FuYu() {
            Disconnect();
    }

    bool Device_Motor_FuYu::Connect() {
    if (m_isConnected) return true;

    std::vector<char> ipBuffer(m_ipAddress.size() + 1);
    strcpy(ipBuffer.data(), m_ipAddress.toStdString().c_str());
    int result = FMC4030_Open_Device(m_deviceId, ipBuffer.data(), m_port);

    if (result == FMC4030_SUCCESS) {
        m_isConnected = true;
        SetStatus(MotorStatus::Connected);
            return true;
         }

    m_isConnected = false;
    SetError(QString("FuYu电机连接失败: %1").arg(result));
    SetStatus(MotorStatus::Disconnected);
        return false;
    }

bool Device_Motor_FuYu::Disconnect() {
    if (m_isConnected) {
        FMC4030_Close_Device(m_deviceId);
        m_isConnected = false;
        SetStatus(MotorStatus::Disconnected);
    }
    return true;
}

bool Device_Motor_FuYu::MoveTo(double position, MotionMode mode) {
    if (!m_isConnected) {
        SetError("设备未连接");
            return false;
        }

        SetStatus(MotorStatus::Moving);

    int fmcMode = (mode == MotionMode::Absolute) ? ABSOLUTE_MOTION : RELATIVE_MOTION;
        int result = FMC4030_Jog_Single_Axis(
            m_deviceId,
            m_axis,
            static_cast<float>(position),
            static_cast<float>(m_currentSpeed),
            static_cast<float>(m_acceleration),
            static_cast<float>(m_deceleration),
        fmcMode
    );

    if (CheckResult(result, "启动移动")) {
        return true;  // 返回启动成功，状态保持Moving
    }

                SetStatus(MotorStatus::Error);
                return false;
            }

double Device_Motor_FuYu::GetPosition() {
    if (!m_isConnected) return m_currentPosition;

    // 使用双精度缓冲区避免栈溢出
    double positionBuffer[2] = {0.0, 0.0};  // 额外缓冲区防止溢出
    float* position = reinterpret_cast<float*>(positionBuffer);

    int result = FMC4030_Get_Axis_Current_Pos(m_deviceId, m_axis, position);

    if (result == FMC4030_SUCCESS) {
        m_currentPosition = static_cast<double>(*position);
    }

    return m_currentPosition;
    }

bool Device_Motor_FuYu::Home(HomeDirection direction) {
        if (!m_isConnected) {
            SetError("设备未连接");
            return false;
        }

        SetStatus(MotorStatus::Homing);

        MotorConfig* config = MotorConfig::getInstance();
        double homeFallStep = config->getHomeFallStep();

        int homeDir = (direction == HomeDirection::Positive) ? HOME_POSITIVE : HOME_NEGATIVE;
        int result = FMC4030_Home_Single_Axis(
           m_deviceId,
           m_axis,
           static_cast<float>(m_currentSpeed),
           static_cast<float>(m_acceleration),
           static_cast<float>(homeFallStep),
           homeDir
       );

        if (CheckResult(result, "启动回零")) {
            return true;  // 返回启动成功，状态保持Homing
        }

        SetStatus(MotorStatus::Error);
        return false;
    }

bool Device_Motor_FuYu::Stop(StopMode mode) {
    if (!m_isConnected) return false;

    int stopMode = (mode == StopMode::Immediate) ? IMMEDIATE_STOP : DECEL_STOP;
    int result = FMC4030_Stop_Single_Axis(m_deviceId, m_axis, stopMode);

    if (CheckResult(result, "Stop")) {
        SetStatus(MotorStatus::Connected);
        return true;
    }
        return false;
    }

bool Device_Motor_FuYu::SetSpeed(double speed) {
    if (Device_Motor::SetSpeed(speed)) {
    return true;
}
            return false;
        }

bool Device_Motor_FuYu::SetAcceleration(double acc) {
    return Device_Motor::SetAcceleration(acc);
}

bool Device_Motor_FuYu::SetDeceleration(double dec) {
    return Device_Motor::SetDeceleration(dec);
}

double Device_Motor_FuYu::GetSpeed() {
    if (!m_isConnected) return Device_Motor::GetSpeed();

    // 使用双精度缓冲区避免栈溢出
    double speedBuffer[2] = {0.0, 0.0};  // 额外缓冲区防止溢出
    float* speed = reinterpret_cast<float*>(speedBuffer);

    int result = FMC4030_Get_Axis_Current_Speed(m_deviceId, m_axis, speed);

    if (result == FMC4030_SUCCESS) {
        return static_cast<double>(*speed);
    }

    return Device_Motor::GetSpeed();
}

bool Device_Motor_FuYu::IsMoving() {
    if (!m_isConnected) return false;

    // 查询FMC4030硬件状态
    // FMC4030_Check_Axis_Is_Stop: 非0=已停止，0=正在运动
    int stopStatus = FMC4030_Check_Axis_Is_Stop(m_deviceId, m_axis);
    bool isCurrentlyMoving = (stopStatus == 0); // 0表示运动中，非0表示停止

    // 根据硬件状态更新内部状态（类似用户例子中的UpdateStatus）
    MotorStatus currentStatus = GetStatus();

    if (isCurrentlyMoving) {
        // 如果硬件显示在运动，但内部状态是Connected，需要判断是什么类型的运动
        if (currentStatus == MotorStatus::Connected) {
            // 无法确定具体类型，默认为Moving（实际应该由调用MoveTo/Home时设置）
            SetStatus(MotorStatus::Moving);
        }
        // 如果已经是Moving或Homing，保持不变
    } else {
        // 如果硬件显示停止，但内部状态还是Moving/Homing，说明操作完成了
        if (currentStatus == MotorStatus::Moving || currentStatus == MotorStatus::Homing) {
            if (currentStatus == MotorStatus::Homing) {
                // 回零完成，位置归零
                m_currentPosition = 0.0;
            }
            SetStatus(MotorStatus::Connected);
        }
    }

    return isCurrentlyMoving;
}

// FuYu特有的配置方法
bool Device_Motor_FuYu::SetAxis(int axis) {
    if (axis >= 0 && axis <= 2) {
            m_axis = axis;
            return true;
    }
    SetError("无效的轴号");
    return false;
}

bool Device_Motor_FuYu::SetDeviceId(int id) {
            m_deviceId = id;
    return true;
}

bool Device_Motor_FuYu::SetIPAddress(const QString& ip) {
            m_ipAddress = ip;
    return true;
}

bool Device_Motor_FuYu::setPort(int port) {
            m_port = port;
    return true;
}



// 辅助方法
bool Device_Motor_FuYu::CheckResult(int result, const QString& operation) {
    if (result == FMC4030_SUCCESS) {
        return true;
    }

    QString errorMsg;
    switch (result) {
        case FMC4030_CONNECT_FAIL:
            errorMsg = "连接失败";
            break;
        case FMC4030_DATA_FAIL:
            errorMsg = "数据建立失败";
            break;
        case FMC4030_SEND_FAIL:
            errorMsg = "数据发送失败";
            break;
        case FMC4030_RECV_FAIL:
            errorMsg = "数据接收失败";
            break;
        case FMC4030_DATA_ERROR:
            errorMsg = "接收数据错误";
            break;
        case FMC4030_NULL_PTR:
            errorMsg = "空指针错误";
            break;
        default:
            errorMsg = "未知错误";
            break;
    }

    SetError(QString("FuYu电机%1失败: %2 (错误码: %3)").arg(operation).arg(errorMsg).arg(result));
    return false;
}